#!/usr/bin/env python3
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_path = get_package_share_directory("controller")
    rviz__file = os.path.join(package_path, "config", "odom_view.rviz")

    runtime = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [
                    FindPackageShare("controller"),
                    "launch",
                    "inner_loop_control.launch.py",
                ]
            )
        )
    )

    play_bag = Node(
        package="ros2bag",
        executable="play",
        name="play_bag",
        output="screen",
        arguments=[
            "database/odom_test",
            "--clock",
            "--loop",
        ],  # --clock表示同步播放 --loop表示循环播放
    )

    record_bag = Node(
        package="ros2bag",
        executable="record",
        name="record_bag",
        output="screen",
        arguments=[
            "-o",
            "database/odom_test",
            "left_speed",
            "right_speed",
            "imu",
        ],  # -a表示录制所有topic，-o表示输出文件名
    )

    odom_node = Node(
        package="controller",
        executable="odom",
        name="real_odom",
        output="screen",
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz__file],
    )

    return LaunchDescription(
        [
            runtime,
            # play_bag,
            # record_bag,
            odom_node,
            rviz2_node,
        ]
    )
